#include "motor.h"


// 电机端口配置
void GPIO_Motor_Config(){
	P1_MODE_IO_PU(GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7);
	P2_MODE_IO_PU(GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3);
}


PWMx_InitDefine pwm_init;
// 电机PWM配置
void PWM_Motor_Config(){
	// PWMA配置
	/*
	  PWM_Period = 24MHz / 1000 -- 用数数来表示时间
	  PWM_Duty = 24MHz / 1000 / 2 -- 用数数来表示时间
	 */
	pwm_init.PWM_Period = PERIOD - 1;		//周期时间,   0~65535
	pwm_init.PWM_DeadTime = 0;	        //死区发生器设置, 0~255
	pwm_init.PWM_CEN_Enable = ENABLE;	//使能计数器, ENABLE,DISABLE
	pwm_init.PWM_MainOutEnable = ENABLE;//主输出使能,  ENABLE,DISABLE
	// PWMA初始化
	PWM_Configuration(PWMA, &pwm_init);
	
	// PWM中断使能
	NVIC_PWM_Init(PWMA, DISABLE, Priority_1);
	
	// PWM切换引脚
	PWM1_SW(PWM1_SW_P20_P21);
	PWM2_SW(PWM2_SW_P22_P23);
	PWM3_SW(PWM3_SW_P14_P15);
	PWM4_SW(PWM4_SW_P16_P17);
}


// 电机初始化
void motor_init(){
	// 拓展寄存器使能
	EAXSFR();
	
	GPIO_Motor_Config();

	PWM_Motor_Config();
}


// 左前轮
void lq_movement(u8 speed, u8 state){
	int duty = SPEED2DUTY(speed) - 1;
	// 前进
	if(state == FORWARD){
		pwm_init.PWM_Mode = CCMRn_PWM_MODE2;
	}
	// 后退
	else if(state == BACKWORD){
		pwm_init.PWM_Mode = CCMRn_PWM_MODE1;
	}
	
	pwm_init.PWM_EnoSelect = ENO4P | ENO4N;
	pwm_init.PWM_Duty = duty;
	PWM_Configuration(PWM4, &pwm_init);
	
	// 停止
	if(state == STOP){
		PWM4P_OUT_DIS();
		PWM4N_OUT_DIS();
	}
}


// 右前轮
void rq_movement(u8 speed, u8 state){
	int duty = SPEED2DUTY(speed) - 1;
	// 前进
	if(state == FORWARD){
		pwm_init.PWM_Mode = CCMRn_PWM_MODE1;
	}
	// 后退
	else if(state == BACKWORD){
		pwm_init.PWM_Mode = CCMRn_PWM_MODE2;
	}
	
	pwm_init.PWM_EnoSelect = ENO3P | ENO3N;
	pwm_init.PWM_Duty = duty;
	PWM_Configuration(PWM3, &pwm_init);
	
	// 停止
	if(state == STOP){
		PWM3P_OUT_DIS();
		PWM3N_OUT_DIS();
	}
}


// 左后轮
void lh_movement(u8 speed, u8 state){
	int duty = SPEED2DUTY(speed) - 1;
	// 前进
	if(state == FORWARD){
		pwm_init.PWM_Mode = CCMRn_PWM_MODE1;
	}
	// 后退
	else if(state == BACKWORD){
		pwm_init.PWM_Mode = CCMRn_PWM_MODE2;
	}
	
	pwm_init.PWM_EnoSelect = ENO1P | ENO1N;
	pwm_init.PWM_Duty = duty;
	PWM_Configuration(PWM1, &pwm_init);
	
	// 停止
	if(state == STOP){
		PWM1P_OUT_DIS();
		PWM1N_OUT_DIS();
	}
}


// 右后轮
void rh_movement(u8 speed, u8 state){
	int duty = SPEED2DUTY(speed) - 1;
	// 前进
	if(state == FORWARD){
		pwm_init.PWM_Mode = CCMRn_PWM_MODE2;
	}
	// 后退
	else if(state == BACKWORD){
		pwm_init.PWM_Mode = CCMRn_PWM_MODE1;
	}
	
	pwm_init.PWM_EnoSelect = ENO2P | ENO2N;
	pwm_init.PWM_Duty = duty;
	PWM_Configuration(PWM2, &pwm_init);
	
	// 停止
	if(state == STOP){
		PWM2P_OUT_DIS();
		PWM2N_OUT_DIS();
	}
}


// 哪个电机前进或后退或停止,速度是多少
void motor_movement(MOTOR_DESC motor){
	// 速度限制在10-100
	if(motor.speed > SPEED_MAX){
		motor.speed == SPEED_MAX;
	}
	if(motor.speed < SPEED_MIN){
		motor.speed == SPEED_MIN;
	}
	
	// 左前轮
	if(motor.motor_no == NO_LQ){
		lq_movement(motor.speed, motor.motor_state);
	}
	// 右前轮
	if(motor.motor_no == NO_RQ){
		rq_movement(motor.speed, motor.motor_state);
	}
	// 左后轮
	if(motor.motor_no == NO_LH){
		lh_movement(motor.speed, motor.motor_state);
	}
	// 右后轮
	if(motor.motor_no == NO_RH){
		rh_movement(motor.speed, motor.motor_state);
	}
}


// 直行 - 全速前进
void motor_go_straight(u8 speed){
	MOTOR_DESC motor;
	// 左边轮全速前进
	motor.motor_no = NO_LQ;
	motor.motor_state = FORWARD;
	motor.speed = speed;
	motor_movement(motor);
	motor.motor_no = NO_LH;
	motor.motor_state = FORWARD;
	motor.speed = speed;
	motor_movement(motor);
	
	// 右边轮全速前进
	motor.motor_no = NO_RQ;
	motor.motor_state = FORWARD;
	motor.speed = speed;
	motor_movement(motor);
	motor.motor_no = NO_RH;
	motor.motor_state = FORWARD;
	motor.speed = speed;
	motor_movement(motor);
}


// 后退 - 全速后退
void motor_go_back(u8 speed){
	MOTOR_DESC motor;
	// 左边轮全速后退
	motor.motor_no = NO_LQ;
	motor.motor_state = BACKWORD;
	motor.speed = speed;
	motor_movement(motor);
	motor.motor_no = NO_LH;
	motor.motor_state = BACKWORD;
	motor.speed = speed;
	motor_movement(motor);
	
	// 右边轮全速后退
	motor.motor_no = NO_RQ;
	motor.motor_state = BACKWORD;
	motor.speed = speed;
	motor_movement(motor);
	motor.motor_no = NO_RH;
	motor.motor_state = BACKWORD;
	motor.speed = speed;
	motor_movement(motor);
}

// 停止
void motor_stop(){
	MOTOR_DESC motor;
	// 左边轮停止
	motor.motor_no = NO_LQ;
	motor.motor_state = STOP;
	motor_movement(motor);
	motor.motor_no = NO_LH;
	motor.motor_state = STOP;
	motor_movement(motor);
	
	// 右边轮停止
	motor.motor_no = NO_RQ;
	motor.motor_state = STOP;
	motor_movement(motor);
	motor.motor_no = NO_RH;
	motor.motor_state = STOP;
	motor_movement(motor);
}


// 左转 - 左边轮不动，右边轮转
void motor_turn_left(u8 speed){
	MOTOR_DESC motor;
	
	// 左边轮不动
	motor.motor_no = NO_LQ;
	motor.motor_state = STOP;
	motor_movement(motor);
	
	motor.motor_no = NO_LH;
	motor.motor_state = STOP;
	motor_movement(motor);
	
	// 右边轮全速前进
	motor.motor_no = NO_RQ;
	motor.motor_state = FORWARD;
	motor.speed = speed;
	motor_movement(motor);
	
	motor.motor_no = NO_RH;
	motor.motor_state = FORWARD;
	motor.speed = speed;
	motor_movement(motor);
}


// 左转 - 柔和版 左边轮慢速前进，右边轮全速前进
void motor_turn_left_slow(u8 speed){
	MOTOR_DESC motor;
	
	// 左边轮慢速前进
	motor.motor_no = NO_LQ;
	motor.motor_state = FORWARD;
	motor.speed = speed / SLOW_TURN_RATIO;
	motor_movement(motor);
	motor.motor_no = NO_LH;
	motor.motor_state = FORWARD;
	motor.speed = speed / SLOW_TURN_RATIO;
	motor_movement(motor);
	
	// 右边轮全速前进
	motor.motor_no = NO_RQ;
	motor.motor_state = FORWARD;
	motor.speed = speed;
	motor_movement(motor);
	motor.motor_no = NO_RH;
	motor.motor_state = FORWARD;
	motor.speed = speed;
	motor_movement(motor);
}


// 右转 - 右边轮不动，左边轮全速前进
void motor_turn_right(u8 speed){
	MOTOR_DESC motor;
	
	// 右边轮不动
	motor.motor_no = NO_RQ;
	motor.motor_state = STOP;
	motor_movement(motor);
	
	motor.motor_no = NO_RH;
	motor.motor_state = STOP;
	motor_movement(motor);
	
	// 左边轮全速前进
	motor.motor_no = NO_LQ;
	motor.motor_state = FORWARD;
	motor.speed = speed;
	motor_movement(motor);
	
	motor.motor_no = NO_LH;
	motor.motor_state = FORWARD;
	motor.speed = speed;
	motor_movement(motor);
}


// 右转 - 柔和版 右边轮慢速前进，左边轮全速前进
void motor_turn_right_slow(u8 speed){
	MOTOR_DESC motor;
	
	// 右边轮慢速前进
	motor.motor_no = NO_RQ;
	motor.motor_state = FORWARD;
	motor.speed = speed / SLOW_TURN_RATIO;
	motor_movement(motor);
	
	motor.motor_no = NO_RH;
	motor.motor_state = FORWARD;
	motor.speed = speed / SLOW_TURN_RATIO;
	motor_movement(motor);
	
	// 左边轮全速前进
	motor.motor_no = NO_LQ;
	motor.motor_state = FORWARD;
	motor.speed = speed;
	motor_movement(motor);
	
	motor.motor_no = NO_LH;
	motor.motor_state = FORWARD;
	motor.speed = speed;
	motor_movement(motor);
}

